#include "pid.h"

float PID_Update(PID_t *pid, float feedback, float setpoint) {
  float error = feedback - setpoint;
  pid->iState += error;
  if (pid->iState > pid->iMAX)
    pid->iState = pid->iMAX;
  else if (pid->iState < pid->iMin)
    pid->iState = pid->iMin;

  float ansI = pid->pid_I * pid->iState;
  float ansD = pid->pid_D * (pid->dState - feedback) * pid->pid_Freq;
  float ansP = pid->pid_P * error;

  pid->dState = feedback;

  return (ansP + ansI + ansD) * pid->pid_K;
}
